#ifndef PID_CONTROL_H_
#define PID_CONTROL_H_


#include <math.h>

namespace control
{
  struct PIDConf
  {
    double kp;
    double ki;
    double kd;
  };
  class PIDControl{
    public:
      PIDControl();
      ~PIDControl();
      
      void init(const PIDConf &pid_conf);
      void setPID(const PIDConf &pid_conf);
      void setIntegralLimit(const double high, const double low);
      void reSet();
      virtual double control(const double error, const double dt);
      int integratorSaturationStatus() const;
      void setIntegratorHold(bool hold);
      void setDiffMode(const int mode );
    
    protected:
      double kp_;
      double ki_;
      double kd_;
      double previous_error_;
      double previous_output_;
      double previous_diff_;
      int diff_mode_;
      double integral_;
      double integrator_saturation_high_;
      double integrator_saturation_low_;
      bool first_hit_;
      bool integrator_enabled_;
      bool integrator_hold_;
      int integrator_saturation_status_;

  };

}//end namespace pnc
#endif
